// 编译时优化的点云配准
template <typename PointT,
          template <typename> class CorrespondenceEstimator,
          template <typename> class TransformEstimator,
          int MaxIterations = 50>
class PointCloudRegistration
{
public:
    using Matrix4 = Eigen::Matrix4f;

    // 配准两个点云
    Matrix4 align(const std::vector<PointT> &source,
                  const std::vector<PointT> &target)
    {
        Matrix4 current_transform = Matrix4::Identity();

        // 创建估计器
        CorrespondenceEstimator<PointT> corr_estimator;
        TransformEstimator<PointT> transform_estimator;

        // 编译时展开的迭代
        return iterate<0>(source, target, current_transform,
                          corr_estimator, transform_estimator);
    }

private:
    // 递归模板实现迭代
    template <int Iteration>
    Matrix4 iterate(const std::vector<PointT> &source,
                    const std::vector<PointT> &target,
                    const Matrix4 &current_transform,
                    CorrespondenceEstimator<PointT> &corr_estimator,
                    TransformEstimator<PointT> &transform_estimator)
    {
        // 基本情况：达到最大迭代次数
        if constexpr (Iteration >= MaxIterations)
        {
            return current_transform;
        }
        else
        {
            // 估计对应关系
            auto correspondences = corr_estimator.estimate(source, target, current_transform);

            // 如果对应关系不足，提前终止
            if (correspondences.size() < 3)
            {
                return current_transform;
            }

            // 估计变换
            Matrix4 transform_delta = transform_estimator.estimate(source, target, correspondences);

            // 更新变换
            Matrix4 new_transform = transform_delta * current_transform;

            // 检查收敛
            float diff = (new_transform - current_transform).norm();
            if (diff < 1e-5)
            {
                return new_transform;
            }

            // 递归下一次迭代
            return iterate<Iteration + 1>(source, target, new_transform,
                                          corr_estimator, transform_estimator);
        }
    }
};

// 使用示例
void register_point_clouds()
{
    std::vector<Point3D> source = load_point_cloud("source.pcd");
    std::vector<Point3D> target = load_point_cloud("target.pcd");

    // 创建配准器，指定对应关系和变换估计方法
    PointCloudRegistration<Point3D,
                           NearestNeighborCorrespondence,
                           SVDTransformEstimation,
                           100>
        registration;

    // 执行配准
    auto transform = registration.align(source, target);
    std::cout << "Final transformation:\n"
              << transform << std::endl;
}